// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include "mpi_motion.h"

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <cmath>
#include <iostream>

#include <franka/exception.h>
#include <franka/robot.h>
#include <chrono>
#include <thread>

/**
 * @example mpi_test.cpp
 * @warning Before executing this example, make sure there is enough space in front of the robot.
 */

int main(int argc, char** argv) {
    if (argc != 2) {
        std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
        return -1;
    }
    franka::Robot robot(argv[1]);
    try {
        setDefaultBehavior(robot);

        std::array<double, 7> q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
        JointMotion<CubicTrajFunc> joint_motion(0.1, q_goal);
        robot.control(joint_motion);

        LineMotion<CubicTrajFunc> motion_generator(4, 0.2, {0, 1, 0});
        robot.control(motion_generator);
        std::cout<<">>> motion finished\n";
        // std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        LineMotion<CubicTrajFunc> motion_generator1(4, -0.2, {0, 1, 0});
        robot.control(motion_generator1);
        std::cout<<">>> motion finished\n";

    } catch (const franka::Exception& e) {
        std::cout << e.what() << std::endl;
        std::cout << "Running error recovery..." << std::endl;
        robot.automaticErrorRecovery();
        return -1;
    }

    return 0;
}

